﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Emgu.CV;
using Emgu.Util;
using System.Diagnostics;
using Emgu.CV.Structure;
using System.Drawing;
using System.Windows.Forms;
using System.Collections;
using System.IO;
namespace NIP.VisionSharp
{
    public class Potraitor
    {
        private Capture cap;
        public int threshold2 { set; get; }
        public int threshold1 { set; get; }
        private int A2_H = 390;
        private int A2_W = 267;
        private  double scaleFactor;
        private double contourRes = 10;
        public DrawState drawState = DrawState.NONE;
        public event EventHandler<DrawEventArgs> ContourReadyDrawEvent;
        private int MAX_CONTOUR = 50;
        private int MIN_CONTOUR = 40;

       


        public Potraitor()
        {
            //MAX_NUMBER_OF_POINT = 20;
            //! initialize Capture object
            if (cap == null)
            {
                try
                {
                    cap = new Capture();
                    cap.SetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FRAME_HEIGHT, 720);
                    cap.SetCaptureProperty(Emgu.CV.CvEnum.CAP_PROP.CV_CAP_PROP_FRAME_WIDTH,1280);
                    
                }
                catch (Exception excpt)
                {

                    throw new Exception(excpt.Message);
                }
            }
            threshold1 = 61;
            threshold2 = 230;
            var statusCheck = new System.Threading.Thread(StatusCheckThread);
            statusCheck.Priority = System.Threading.ThreadPriority.Lowest;
            statusCheck.Start();
        }
        public Bitmap ImageCapture()
        {
            if (cap == null)
            {
                try
                {
                    cap = new Capture();
                }
                catch
                {
                    Debug.WriteLine("Error in capture");
                    throw;
                }
            }
            else
            {
                //return ExtractFaceFromFrame().ToBitmap();
                return GetContour();
                //return CielabChannelMaskBGSubtraction();
            }
            return null;
        }



        public Image<Bgr, Byte> ExtractFaceFromFrame(Image<Bgr, Byte> rawImage)
        {
            try
            {

                Image<Gray, Byte> gray = rawImage.Convert<Gray, Byte>(); //Convert it to Grayscale
               

                gray._EqualizeHist();

                //Read the HaarCascade objects
                HaarCascade face = new HaarCascade("haarcascade_frontalface_default.xml");
 

                //Detect the faces  from the gray scale image and store the locations as rectangle
                //The first dimensional is the channel
                //The second dimension is the index of the rectangle in the specific channel
                MCvAvgComp[][] facesDetected = gray.DetectHaarCascade(
                   face,
                   1.1,
                   10,
                   Emgu.CV.CvEnum.HAAR_DETECTION_TYPE.DO_CANNY_PRUNING,
                   new Size(20, 20));

                foreach (MCvAvgComp f in facesDetected[0])
                {
                    //draw the face detected in the 0th (gray) channel with blue color
                    //rawImage.Draw(f.rect, new Bgr(Color.Blue), 2);
                    //var rec = f.rect;

                    //Set the region of interest on the faces
                    rawImage.ROI = f.rect;
                   
                }
                return rawImage.Copy();

            }
            catch (Exception exp)
            {
                throw new Exception(exp.Message);
            }

        }
        
        public Bitmap GetContour()
        {
            Contour<Point> contours;
            Image<Bgr, Byte> frame;
            int color = 0;
            int step = 30;
            double[] lastPoint = new double[2];
            List<List<double[]>> list = new List<List<double[]>>();
            int deletedNo = 0;
            List<double[]> aList;

            try
            {
                var count = 0;

                do
                {
                    frame = cap.QueryFrame();
                    frame = cap.QueryFrame();

                    Image<Bgr, Byte> face = ExtractFaceFromFrame(frame);

                    double ratioh = A2_H / Convert.ToDouble(face.Size.Height);
                    double ratiow = A2_W / Convert.ToDouble(face.Size.Width);
                    //Debug.WriteLine("Ratioh, Ratiow:" + ratioh + ":" + ratiow);
                    //Debug.WriteLine("A2_H/2, A2_W/2:" + (A2_W / 2.0) + "," + (A2_H / 2.0));
                    scaleFactor = (ratioh > ratiow) ? ratiow : ratioh;

                    // frame.Resize(0.5, CV_INTER_LINEAR);
                    Image<Gray, Byte> gray = face.Convert<Gray, Byte>();
                    Image<Gray, Byte> canny = new Image<Gray, byte>(gray.Size);
                    MemStorage stor = new MemStorage();

                    CvInvoke.cvCanny(gray, canny, threshold1, threshold2, 3);



                    canny.Dilate(6);
                    canny.Erode(6);
                    canny.SmoothGaussian(9);
                    canny.SmoothBlur(10, 10);
                    contours = canny.FindContours(
                     Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE,
                     Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_TREE,
                     stor);
                    //Debug.WriteLine("total Point in Contour" + contours.Total.ToString());
                    frame.SetZero();

                  

                    //return CielabChannelMaskBGSubtraction(frame).ToBitmap();

                    //frame = frame.Sub(background, CielabChannelMaskBGSubtraction(frame));
                    //Image<Gray, Byte> gray = frame.Convert<Gray, Byte>();
                    //var contours = gray.FindContours();
                    //gray.Draw(contours, new Gray(), -1);
                   

                    if (contours != null)
                    {
                        Debug.WriteLine("Contour Found");
                       list = new List<List<double[]>>();
                       count = 0;
                       // Debug.WriteLine("total Point in Contour" + contours.Total.ToString());
                        for (Contour<Point> c = contours; c != null; c = c.HNext)
                        {
                            aList = new List<double[]>();
                            deletedNo = 0;
                            ++count;
                           // Debug.WriteLine("Find Contour  " + ++count + "  Total  " + c.Total);
                            frame.Draw(c, new Bgr((count % 2) * step, (count % 3) * step, (count % 4) * step), new Bgr(Color.Red), 1,1, new Point(0, 0));
                            for (int i = 0; i < c.Total; i++)
                            {
                            
                                var aPoint = ConvertToWorldCoordinate(c.ElementAt(i));
                                if (i == 0)
                                {
                                    //Debug.WriteLine("Point Coordinate X Y World  " + aPoint.ToString());
                                    aList.Add(aPoint);
                                }
                                else
                                {                  
                                    lastPoint = aList.Last() ;
                                    if (Math.Sqrt(Math.Pow(aPoint[0] - lastPoint[0], 2) + Math.Pow(aPoint[1] - lastPoint[1], 2)) > contourRes)
                                    {
                                       // Debug.WriteLine("Point Coordinate X Y World  " + aPoint[0] + "," + aPoint[1]);
                                        aList.Add(aPoint);
                                    }
                                    else
                                    {
                                        deletedNo++;
                                    }
                                }
                            
                           
                            }
                            //Debug.WriteLine("Deleted   " + deletedNo);
                            list.Add(aList);
                        
                          }

                         Debug.WriteLine(" New Image Taken");
                     }
                } while (list.Count > MAX_CONTOUR || list.Count < MIN_CONTOUR);
                Debug.WriteLine(" Total count" + list.Count);
                var args = new DrawEventArgs();
                args.PointList = list;

                string strLogText = "New contour";

                // Create a writer and open the file:
                StreamWriter log;

                log = new StreamWriter("contour.txt");
                //if (!File.Exists("contour.txt"))
                //{
                //    log = new StreamWriter("contour.txt");
                //}
                //else
                //{
                //    log = File.AppendText("contour.txt");
                //}

                // Write to the file:
                log.WriteLine(DateTime.Now);
                log.WriteLine(strLogText);
                int l = list.Count;
                Debug.WriteLine("contour list length " + l);
                for (int ll = 0; ll < l; ll++)
                {
                    log.WriteLine("Contour " + ll);
                    int cc = list[ll].Count;
                    for (int ccc = 0; ccc < cc; ccc++)
                    {
                        log.WriteLine(String.Format("{0},{1}", Convert.ToInt16(list[ll][ccc][0]), Convert.ToInt16(list[ll][ccc][1])));
                        //Debug.WriteLine(String.Format("{0},{1}", Convert.ToInt16(list[ll][ccc][0]), Convert.ToInt16(list[ll][ccc][1])));
                    }
                    
                }


                // Close the stream:
                log.Close();
                args.image = frame.ToBitmap();
                args.image.Save("contour.png");
                if (ContourReadyDrawEvent != null)
                {
                    ContourReadyDrawEvent(new object(), args);
                    drawState = DrawState.IMAGE_PROCESSING_DONE_ROBOT_IN_ACTION;

                }
                
                //return args;
            }
            catch (Exception exp)
            {
                throw new Exception(exp.Message);
            }
            return null;
        }

        private double[] ConvertToWorldCoordinate(Point point)
        {
            //return new double[] { (point.X * scaleFactor) - (A2_W / 2.0), (point.Y * scaleFactor) - (A2_H / 2.0) };
            return new double[] { (point.X * scaleFactor), (point.Y * scaleFactor) };
        }
        public Bitmap CaptureBackground()
        {
            try
            {
                
            }
            catch (Exception exp)
            {
                throw new Exception(exp.Message);
            }
            return null;
        }

        private Image<Gray, byte> returnChangedMask(Image<Bgr, byte> frame, Image<Bgr, byte> referenceImage, double threshold)
        {
            if (frame.Width != referenceImage.Width || frame.Height != referenceImage.Height)
            {
                return null;
            }

            Image<Gray, byte> output;
            Image<Gray, byte> frameTmp = frame.Convert<Gray, byte>();
            Image<Gray, byte> referenceTmp = referenceImage.Convert<Gray, byte>();
            frameTmp.SmoothBlur(5, 5);
            referenceTmp.SmoothBlur(5, 5);
            //Calc absolute difference 
            output = frameTmp.AbsDiff(referenceTmp).ThresholdBinary(new Gray(threshold), new Gray(255));

            frameTmp.Dispose();
            referenceTmp.Dispose();

            return output;
        }

        private Image<Gray, Byte> CielabChannelMaskBGSubtraction(Image<Bgr, Byte> rgb)
        {
            double threshold = 145;
            //Image<Bgr, byte> rgb = capture.QueryFrame();
            rgb = cap.QueryFrame();
            Image<Lab, Byte> img = rgb.Convert<Lab, Byte>();
            //get the a* channel 
            Image<Gray, Byte> gray = img[2];
 
            //threshold and invert
            gray = gray.ThresholdBinary(new Gray(threshold), new Gray(255));
            return gray;
        }

        public void SpeechActionRequest_Handler(object sender, Speech.EventArgs.SpeechActionRequestedEventArgs args)
        {
            if (args.command == DynamicControl.ControlCommands.DRAW)
            {
                drawState = DrawState.COMMAND_RECEIVED_AWAITING_ROBOT_IN_POSITION;
            }
        }

        public void RobotMovementCompleted_Handler(object sender, DynamicControl.RobotStatusUpdateEventArgs args)
        {
            if (args.status.Contains("photo") && args.status.Contains("position"))
            {
                drawState = DrawState.IN_POSITION_IMAGE_PROCESSING;
                GetContour();
            }
            else if (args.status.Contains("draw") && args.status.Contains("complete"))
            {
                drawState = DrawState.COMPLETED;
            }
        }

        public void CommandCompletedRobotStatusUpdateHandler(object sender, DynamicControl.RobotStatusUpdateEventArgs args)
        {

            if (args.completedCommand == DynamicControl.ControlCommands.TAKE_PHOTO)
            {
                drawState = DrawState.IN_POSITION_IMAGE_PROCESSING;
                return;
            }
            return;
            //! Contour Completed EVENT has not been implemented
            //else if (args.status.Contains("draw") && args.status.Contains("complete"))
            //{
            //    drawState = DrawState.COMPLETED;
            //}
        }

        public void StatusCheckThread()
        {
            while (true)
            {
                System.Threading.Thread.Sleep(1000);
                if (drawState == DrawState.IN_POSITION_IMAGE_PROCESSING)
                {
                    GetContour();
                }
            }
        }

    }
}
